#include <18f2550.h>

//seccion de configuracion del microcontrolador
#fuses NOMCLR        // Pin MCLR como entrada digital RE3.
#fuses HSPLL         // Cristal HS + PLL.
#fuses NOWDT         // No activamos el WatchDog.
#fuses NOPROTECT     // No activamos la protección contra lecturas.
#fuses NOLVP         // No activamos el modo de programación a bajo voltaje.
#fuses NODEBUG       // No incluimos código para debug.
#fuses USBDIV        // 96Mhz del PLL/2 = 48Mhz para USB en Full speed.
#fuses PLL5          // Factor de división de la entrada del PLL = 1 para 4mhz.
#fuses CPUDIV1       // El PLL postscaler decide la división en 2 de la frecuencia de salida del PLL de 96MHZ.
#fuses VREGEN      // Desactivamos el regulador de 3.3v del puerto USB.
#fuses NOPBADEN      // Puerto B como I/O digital.
#use delay(clock=48000000) // Frecuencia de trabajo 48Mhz.


//seccion de inclusion de librerias
#include "usb_cdc.h"
#include "usb_desc_cdc.h"
#include <string.h>
#include <stdlib.h>


#priority timer1,rda




//Definiciones de variables globales
int const lenbuff=10; //tama�o del buffer para recibir la instrucci�n
int xbuff=0; //Este es el indice, siguiente caracter en el buffer
char cbuff[lenbuff]; //Buffer de recepci�n
int1 flagcommand; //bandera para saber si presionamos enter
char rcvchar= 0x00; //ultimo caracter recibido  <--- Declaracion


#define SERVO1 PIN_B0
#define SERVO2 PIN_B1
#define SERVO3 PIN_B3
#define SERVO4 PIN_B4
#define SERVO5 PIN_B5
#define SERVO6 PIN_B6
#define SERVO7 PIN_B7
#define SERVO8 PIN_A4

const int16 Ticks4Window = 12500; // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int16 Ticks4Minimum = 3500; // PWM High for Minimum Position = 0.7 ms
const int16 Ticks4Center = 7426; // PWM High for Center Position = 1.5 ms
const int16 Ticks4Maximum = 11500; // PWM High for Maximum Position = 2.3 ms


//Debe de llevar todo el arreglo un valor asignado, ya que si no es así
//el timer1 se cargará de forma inmediata con el valor 65535, causando
//un desborde de 200ns lo cual impedirá que trabaje de forma adecuada 
//la temporización.
static int16 Servo_PWM[8]={Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center};
static int8 Servo_Idx=0;
static int1 SERVO1_ON=1;
static int1 SERVO2_ON=1;
static int1 SERVO3_ON=1;
static int1 SERVO4_ON=1;
static int1 SERVO5_ON=1;
static int1 SERVO6_ON=0;
static int1 SERVO7_ON=0;
static int1 SERVO8_ON=0;
static int1 flag_Phase;
static int16 Ticks4NextInterrupt=53035;
int16 posI;

   

#int_timer1
void timer1_isr(void){

  if(flag_Phase==0){
    if(Servo_Idx==0 && SERVO1_ON) output_high(SERVO1);
    if(Servo_Idx==1 && SERVO2_ON) output_high(SERVO2);
    if(Servo_Idx==2 && SERVO3_ON) output_high(SERVO3);
    if(Servo_Idx==3 && SERVO4_ON) output_high(SERVO4);
    if(Servo_Idx==4 && SERVO5_ON) output_high(SERVO5);
    if(Servo_Idx==5 && SERVO6_ON) output_high(SERVO6);
    if(Servo_Idx==6 && SERVO7_ON) output_high(SERVO7);
    if(Servo_Idx==7 && SERVO8_ON) output_high(SERVO8);
    Ticks4NextInterrupt = 65535 - Servo_PWM[Servo_Idx];
    set_timer1(Ticks4NextInterrupt);
  }
  if(flag_Phase==1){
    if(Servo_Idx==0 && SERVO1_ON) output_low(SERVO1);
    if(Servo_Idx==1 && SERVO2_ON) output_low(SERVO2);
    if(Servo_Idx==2 && SERVO3_ON) output_low(SERVO3);
    if(Servo_Idx==3 && SERVO4_ON) output_low(SERVO4);
    if(Servo_Idx==4 && SERVO5_ON) output_low(SERVO5);
    if(Servo_Idx==5 && SERVO6_ON) output_low(SERVO6);
    if(Servo_Idx==6 && SERVO7_ON) output_low(SERVO7);
    if(Servo_Idx==7 && SERVO8_ON) output_low(SERVO8);
    Ticks4NextInterrupt = 65535 - Ticks4Window + Servo_PWM[Servo_Idx];
    set_timer1(Ticks4NextInterrupt);
    if(++Servo_Idx>7) Servo_Idx=0;  //tenia >7
  }
  ++flag_Phase;
}

//Prototipos
void serial_isr();
void initBuff(void);
void add2cbuff(char c);
void echosel(char c);
void command_menu(void);
void command_process(void);


void main(void) {

// Inicializando el sistema
  disable_interrupts(global);
  setup_adc_ports(NO_ANALOGS);
  setup_adc(ADC_OFF);
  setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
  setup_timer_0(RTCC_OFF);
  setup_timer_1(T1_INTERNAL | T1_DIV_BY_1);
  setup_timer_2(T2_DISABLED,0,1);
  setup_timer_3(T3_DISABLED);
  port_b_pullups(FALSE);


  usb_cdc_init();
  usb_init();  
  
/*  Debo revisar si esta linea a�n es v�lida
  set_tris_a(0b00000000);   
  set_tris_b(0b00000000);
  */   
  
  initBuff(); //inicializamos el buffer
  
  //Inicializamos las salidas de los servos
  output_low(SERVO1);
  output_low(SERVO2);
  output_low(SERVO3);
  output_low(SERVO4);
  output_low(SERVO5);
  output_low(SERVO6);
  output_low(SERVO7);
  output_low(SERVO8);

  delay_ms(300);
  set_timer1(Ticks4NextInterrupt);
  enable_interrupts(int_timer1);
  enable_interrupts(global);
  posI=Ticks4Center;    //manda al centro a todos los servos


  while(!usb_cdc_connected())
  {
	  //Encender led que indique que esta alimentado el circuito
	  //pero que no ha sido conectado al USB
	  }
   //Colocar otro led que nos indique que ya ha sido conectado
   //el dispositivo a nuestro puerto USB
   
   while(TRUE)
   {
   usb_task();			//Inicializando tareas USB
   if(usb_enumerated())		//Esperar a que sea enumerado el dispositivo
   {   
   command_menu();
      while(TRUE)
     {
	do{
	if(usb_cdc_kbhit())
	{
		serial_isr();
	}
	}while(flagcommand=='\0');

	if(flagcommand!='\0')
	{
        command_process();
	}
	else
	{
	flagcommand='\0';
	}
      }
   }
   }


}

//Funcion de vaciado del buffer
void initBuff(void){
   int i;
   for (i=0; i< lenbuff;i++){
         cbuff[i]=0x0;
      }xbuff=0x00;
}


//agrega un caracter mas al buffer, o realiza un acción
void add2cbuff(char c){
if(xbuff<lenbuff){  //Añade caracteres al buffer mientras no se llene el buffer
   switch(c){
   case 0x0D: //Enter  --> Habilita flag para procesar comando
            flagcommand=1;
            break;
   case 0x08: //BackSpace --> Borra del buffer el ultimo caracter insertado
            if(xbuff>0) cbuff[--xbuff]=0x00;
            break;
   case 0x1B: //ESC  --> Borra todo el buffer
            initBuff();
            break;
   default:  //añade caracter al buffer recibido
         cbuff[xbuff++]=c;
   }
}
else{
initBuff();//Si el buffer esta lleno, vacialo
printf(usb_cdc_putc,"Buff. Lleno\r\n");  //Envia mensaje de que el buffer esta lleno
}
}

//Solo imprime los caracteres imprimibles
void echosel(char c){
switch(c){
      case 0x0D: //Si presionamos la tecla intro
     // printf("\r\n[ENT]\r\n ");
      break;
      case 0x08: //Si presionamos la tecla BackSpace
     // printf("\r\n [BACK]\r\n");
      break;
      case 0x1B: //Si presionamos la tecla ESC
     // printf("\r\n[ESC]\r\n");
      break;
      default:
      usb_cdc_putc(c); //Echo de cualquier otro caracter
   }
}


void serial_isr(){  // interrupcion serial usart
char rcvchar= 0x00; //ultimo caracter recibido  <--- Declaracion


      rcvchar=usb_cdc_getc(); //se carga a la variable rcvchar
      add2cbuff(rcvchar); //y se agrega al buffer
      echosel(rcvchar); //hacer un eco selectivo
      

 }

//Procesador de comandos
void command_menu(void){
printf(usb_cdc_putc,"\r\nComunicaciones\r\n");
printf(usb_cdc_putc,"Metodo: Recibiendo sobre el buffer\r\n\n");
printf(usb_cdc_putc,"\r\n\r\n");
}

void command_process(void){

/*
El formato de los comandos es el siguiente:
[a...e] [123] [xx]
servo posicion velocidad
primer byte un caracter alfabético de entre a y e que define al servo
tres bytes para definir la posicion del servo
dos bytes para definir la velocidad de movimiento del servo
a12301 {enter}
mueve el servo 1 a 123 grados de la posicion de origen a la velocidad 01
*/
   int16 i;
   int1 DerIzq = 1; //bandera para saber si va a 180 o a 0 grados
   // DerIzq = 1   Indicará que vamos de 0 a 180 por tanto suma
   // DerIzq = 0   Indicará que vamos de 180 a 0 por tanto resta
   char cmd[lenbuff]; //comando tecleado
   char servo[2];
   char pos[4];  //para almacenar las posiciones
   int8 posicion;
   char velocidad[4];  //para almacenar los incrementos
   int8 vel;
   
   int16 posF;
   int16 vueltas;
   
   flagcommand=0; //Es desactivada la flag cuando el comando esta pendiente
   printf(usb_cdc_putc,"procesando ...\r\n");
   
   strcpy(cmd,cbuff); //hacer la copia para procesar y almacenar variables
   strncpy(servo,cbuff,1);
   printf(usb_cdc_putc,"%s \r\n",servo);
   //Esta condicional sirve para asignar el valor del valor inicial
   //ke en diferencia con el valor final moveran al servo en 
   //uno o en otro sentido.
   switch(servo[0]){
   case 'a':
            posI = servo_PWM[0];
            break;
   case 'b':
            posI = servo_PWM[1];
            break;
   case 'c':
            posI = servo_PWM[2];
            break;
   case 'd':
            posI = servo_PWM[3];
            break;
   case 'e':
            posI = servo_PWM[4];
            break;
   default:
            printf(usb_cdc_putc,"opcion no valida vuelve a intentar");
            break;
   }
   //guarda la posicion del servo
   strcpy(cmd,cbuff); 
   for(i=1;i<4;i++){
   pos[i-1]= cmd[i];
   }
   posicion = atoi(pos);
   printf(usb_cdc_putc,"%u \r\n",posicion);
   
   //guarda la velocidad con la que se moverá el servo
   strcpy(cmd,cbuff);
   for(i=4;i<7;i++)
   {
   velocidad[i-4]=cmd[i];
   }
   vel = atoi(velocidad);
   printf(usb_cdc_putc,"%u\r\n", vel);
   
   //Determinamos el numero de vueltas
/*   3400   -- 0000 --- 0.03
   3398
   const int16 Ticks4Window = 12500; // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int16 Ticks4Minimum = 3500; // PWM High for Minimum Position = 0.7 ms
const int16 Ticks4Center = 7399; // PWM High for Center Position = 1.5 ms
const int16 Ticks4Maximum = 11500; // PWM High for Maximum Position = 2.3 ms
   si 7399 corresponde a 90° entonces 180° es a 14798  en teoria 
   un grado corresponde a un incremento de 27
   Dado que cada incremento de 27 nos da un adelanto de 27 entonces tenemos que
   el maximo es 12259
   */
   posF=44.4*posicion+3500;
   
   if(posF>posI){
    vueltas = posF - posI;
    vueltas = vueltas / vel;
    DerIzq = 1;
   }
   else
   {
    vueltas = posI - posF;
    vueltas = vueltas/ vel;
    DerIzq = 0;
   }
   
//---------------------------------------------------------------------
   if(servo[0]=='x') {
   output_high(PIN_A5);  //pin de hablitacion
   output_high(PIN_A0);  //mueve a la izquierda es para la pinza
   }
   if(servo[0]=='y'){
   output_low(PIN_A5); //pin de hablitacion
   output_low(PIN_A0); //detiene a la pinza
   }
   if(servo[0]=='z'){
   output_high(PIN_A5);  //pinde hablitacion
   output_low(PIN_A0);   //pin mover a la derecha
   }
//--------------------------------------------------------------------------   
   printf(usb_cdc_putc,"posI: %lu \r\n",posI);
   printf(usb_cdc_putc,"posF: %lu \r\n",posF);
   printf(usb_cdc_putc,"vueltas: %lu \r\n",vueltas);

/*
   Debe de leerse el valor que tiene del servo desde el array
   ´para determinar el valor inicial y no unicamente dejarlo
   como valor final. esto causa el eeror de que al mover
   multiples motores con una unica variable se moveran en conse-
   cuencia del anterior
*/
  // posI = posF;  Esto no es correcto
   
   if(DerIzq)
   {
         switch(servo[0])
         {
         case 'a':  //mueve al servo uno en incrementos de vel hasta llegar a vueltas
            for(i=0;i<vueltas;i++){
            Servo_PWM[0]+=vel;
            delay_ms(1);}
            break;
         case 'b':  //mueve al servo dos en incrementos de vel hasta llegar a vueltas
            for(i=0;i<vueltas;i++){
            Servo_PWM[1]+=vel;
            delay_ms(1);}
            break;
         case 'c':
            for(i=0;i<vueltas;i++){
            Servo_PWM[2]+=vel;
            delay_ms(1);}
            break;
         case 'd':
            for(i=0;i<vueltas;i++){
            Servo_PWM[3]+=vel;
            delay_ms(1);}
            break;
         case 'e':
            for(i=0;i<vueltas;i++){
            Servo_PWM[4]+=vel;
            delay_ms(1);}
            break;
         default:
            printf(usb_cdc_putc,"X_x\r\n");
         }
   }
   else{
         switch(servo[0])
         {
            case 'a':  //mueve al servo uno en incrementos de vel hasta llegar a vueltas
                  for(i=0;i<vueltas;i++){
                  Servo_PWM[0]-=vel;
                  delay_ms(1);}
                  break;
            case 'b':  //mueve al servo dos en incrementos de vel hasta llegar a vueltas
                  for(i=0;i<vueltas;i++){
                  Servo_PWM[1]-=vel;
                  delay_ms(1);}
                  break;
         case 'c':
            for(i=0;i<vueltas;i++){
            Servo_PWM[2]-=vel;
            delay_ms(1);}
            break;
         case 'd':
            for(i=0;i<vueltas;i++){
            Servo_PWM[3]-=vel;
            delay_ms(1);}
            break;
         case 'e':
            for(i=0;i<vueltas;i++){
            Servo_PWM[4]-=vel;
            delay_ms(1);}
            break;
            default:
                  printf(usb_cdc_putc,"X_x\r\n");
          }   
   }
   
   
   initBuff();  //inicializamos el buffer
   printf(usb_cdc_putc,"Procesado\r\n\r\n");
   }

